/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/include/agvNavigate/publicInterfaceInc/lqr_controller.h
 * @Description  : LQR控制器类和相关数据结构，用于实现基于线性二次调节器的控制算法。
 *                 主要包括状态、速度和路径的结构体定义，以及LQRController类的声明，负责计算控制输入以优化机器人状态。 
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 16:27:31
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/ 
#pragma once

//#include <Eigen/Dense>
#include <eigen3/Eigen/Eigen>
#include <vector>
#include <iostream>
#include "rclcpp/rclcpp.hpp"
#include "geometry.h"
#include <math.h>
// #include <corecrt_math_defines.h>

using namespace std;
using namespace Eigen;

// class Speed {
// public:
//     Speed() {};
//     Speed(double v, double w) : v(v), w(w) {};
//     double v;
//     double w;
// };
// class Pos {
// public:
//     Pos() {};
//     Pos(double x, double y, double yaw) : x(x), y(y), yaw(CAngle::NormAngle2(yaw)) {};

//     double x;
//     double y;
//     double yaw;
// };

// class Path {
// public:
//     Path() {};
//     Path(Pos pos, Speed speed) :speed(speed), pos(pos) {};

//     Pos pos;
//     Speed speed;
// };

struct Pos {
    double x = 0.0;
    double y = 0.0;
    double yaw = 0.0;   
};
struct Speed {
    double v = 0.0;
    double w = 0.0;   
};
struct State {
    Pos pos;
    Speed speed;
};

// class RobotModel//TEST
// {
// public:
//     RobotModel() {};
//     RobotModel(Pos& pos, Speed& speed, double dt) :speed(speed), pos(pos), dt(dt) {};
//     ~RobotModel() {};
//     void Update(Speed& speed_) 
//     {
//         pos.yaw = CAngle::NormAngle2(pos.yaw + speed_.w * dt);
//         //pos.yaw = pos.yaw + speed_.w * dt;
//         pos.x = pos.x + speed_.v * cos(pos.yaw) * dt;
//         pos.y = pos.y + speed_.v * sin(pos.yaw) * dt;
//         speed.w = speed_.w;
//         speed.v = speed_.v;
//     };

//     Path GetRobotState()
//     {
//         Path robotstate(pos, speed);
//         return robotstate;
//     };
//     //private:
//     Pos pos;
//     Speed speed;
//     double dt;
// };

class LQRController {
public:
    //LQRController();
    LQRController(int n, double dt);
    ~LQRController() = default;
    Speed LQRControl(State& robot_state, State& pre_point);
    
    double qx_, qy_, qtheta_, rv_, rw_, pre_point_dist_, eps_;
    int iterations_;   
private:
    MatrixXd CalculateRicatti();
    void CalculateAB(State& pre_point);
    void UpdateGain();

    int n_ = 50;
    MatrixXd A_;  // 系统状态转移矩阵
    MatrixXd B_;  // 输入控制矩阵
    MatrixXd Q_;  // 状态成本权重矩阵
    MatrixXd R_;  // 控制成本权重矩阵
    double dt_;
    double precision_;
};
 
